function rocket_guidance()
    % 参数设置
    T = 1000;       % 推力 (N)
    m = 1000;       % 质量 (kg)
    g = [0; -9.81]; % 重力加速度 (m/s²)
    t0 = 0;         % 初始时间
    tf = 100;       % 终端时间 (s)
    r0 = [0; 0];    % 初始位置
    v0 = [0; 0];    % 初始速度
    rd = [1000; 2000]; % 目标位置
    vd = [100; 0];     % 目标速度

    % 牛顿迭代参数
    lambda0_guess = [0; 0; 0; 0]; % 初始协态猜测 [λ_x0; λ_y0; λ_vx0; λ_vy0]
    max_iter = 20;                % 最大迭代次数
    tolerance = 1e-6;             % 收敛容差
    delta = 1e-5;                 % 有限差分步长

    for iter = 1:max_iter
        % 积分轨迹
        Y0 = [r0; v0; lambda0_guess];
        [~, Y] = ode45(@(t,Y) rocket_odes(t, Y, T, m, g), [t0 tf], Y0);
        
        % 计算终端误差
        r_tf = Y(end, 1:2)';
        v_tf = Y(end, 3:4)';
        error = [r_tf - rd; v_tf - vd];
        if norm(error) < tolerance
            fprintf('Converged after %d iterations.\n', iter);
            break;
        end
        
        % 计算雅可比矩阵 (有限差分)
        J = zeros(4,4);
        for i = 1:4
            lambda_perturbed = lambda0_guess;
            lambda_perturbed(i) = lambda_perturbed(i) + delta;
            Y0_perturbed = [r0; v0; lambda_perturbed];
            [~, Y_perturbed] = ode45(@(t,Y) rocket_odes(t, Y, T, m, g), [t0 tf], Y0_perturbed);
            r_tf_perturbed = Y_perturbed(end, 1:2)';
            v_tf_perturbed = Y_perturbed(end, 3:4)';
            error_perturbed = [r_tf_perturbed - rd; v_tf_perturbed - vd];
            J(:,i) = (error_perturbed - error) / delta;
        end
        
        % 牛顿更新
        lambda0_guess = lambda0_guess - J \ error;
    end

    % 输出结果
    [t, Y] = ode45(@(t,Y) rocket_odes(t, Y, T, m, g), [t0 tf], [r0; v0; lambda0_guess]);
    plot_trajectory(t, Y, rd, vd);
end

function dYdt = rocket_odes(t, Y, T, m, g)
    % 状态和协态变量分解
    lambda_x = Y(5); lambda_y = Y(6);
    lambda_vx = Y(7); lambda_vy = Y(8);
    
    % 推力方向计算
    theta = atan2(-lambda_vy, -lambda_vx); % 最优控制方向
    thrust = T/m * [cos(theta); sin(theta)];
    
    % 状态方程
    dYdt = zeros(8,1);
    dYdt(1:2) = Y(3:4);                % dr/dt = v
    dYdt(3:4) = thrust + g;             % dv/dt = thrust/m + g
    dYdt(5:6) = [0; 0];                 % dλ_r/dt = 0
    dYdt(7:8) = -[lambda_x; lambda_y];  % dλ_v/dt = -λ_r
end

function plot_trajectory(t, Y, rd, vd)
    figure;
    subplot(2,1,1);
    plot(Y(:,1), Y(:,2), 'b-', rd(1), rd(2), 'ro');
    xlabel('X'); ylabel('Y'); title('轨迹');
    legend('轨迹', '目标位置');
    
    subplot(2,1,2);
    plot(t, Y(:,3), 'r-', t, Y(:,4), 'b-');
    xlabel('时间'); ylabel('速度');
    legend('Vx', 'Vy');
end